#include "CodeBook.h"

CodeBook::CodeBook()
{
	m_numEntries = 0;
	m_time = 0;
}

CodeBook::~CodeBook()
{

}

float CodeBook::ColorDist(const uchar *pixel, const CodeWord &cw)
{
	float intensity = float(pixel[0]*pixel[0] + pixel[1]*pixel[1] + pixel[2]*pixel[2]);
	float intensityWord = cw.valueRGB[0]*cw.valueRGB[0] + cw.valueRGB[1]*cw.valueRGB[1] + cw.valueRGB[2]*cw.valueRGB[2];
	float intensityCross = (pixel[0]*cw.valueRGB[0] + pixel[1]*cw.valueRGB[1] + pixel[2]*cw.valueRGB[2]) *
		(pixel[0]*cw.valueRGB[0] + pixel[1]*cw.valueRGB[1] + pixel[2]*cw.valueRGB[2]);
	return sqrt(intensity - intensityCross / intensityWord);
}

bool CodeBook::Brightness(const CodeWord &cw, float inten, float alph, float beta)
{

	float lowIntensity = alph * cw.maxIntensity;
	float highIntensity = beta*cw.maxIntensity < cw.minIntensity/alph?beta*cw.maxIntensity : cw.minIntensity/alph;
	if(inten >= lowIntensity && inten <= highIntensity)
		return true;
	else
		return false;
}

void CodeBook::UpdateCodeBook(const uchar *pixel, float threshhold, float alph, float beta)
{
	m_time++;
	float inten = sqrt(float(pixel[0]*pixel[0] + pixel[1]*pixel[1] + pixel[2]*pixel[2]));
	int i;
	for(i = 0; i < m_numEntries; i++)
	{
		float delta = ColorDist(pixel, *m_codeWords[i]);//cout<<delta<<endl;
		if(delta <= threshhold && Brightness(*m_codeWords[i], inten, alph, beta))
		{
			m_codeWords[i]->valueRGB[0] = (m_codeWords[i]->frequency*m_codeWords[i]->valueRGB[0] + pixel[0]) / (m_codeWords[i]->frequency + 1);
			m_codeWords[i]->valueRGB[1] = (m_codeWords[i]->frequency*m_codeWords[i]->valueRGB[1] + pixel[1]) / (m_codeWords[i]->frequency + 1);
			m_codeWords[i]->valueRGB[2] = (m_codeWords[i]->frequency*m_codeWords[i]->valueRGB[2] + pixel[2]) / (m_codeWords[i]->frequency + 1);
			m_codeWords[i]->minIntensity = inten < m_codeWords[i]->minIntensity?inten : m_codeWords[i]->minIntensity;
			m_codeWords[i]->maxIntensity = inten > m_codeWords[i]->maxIntensity?inten : m_codeWords[i]->maxIntensity;
			m_codeWords[i]->frequency += 1;
			m_codeWords[i]->maxNegRunLen = m_codeWords[i]->maxNegRunLen > (m_time-m_codeWords[i]->lastVisity)?m_codeWords[i]->maxIntensity : (m_time-m_codeWords[i]->lastVisity);
			m_codeWords[i]->lastVisity = m_time;
			break;
		}
	}
	if(i >= m_numEntries)
	{
		CodeWord **codeWords = new CodeWord *[m_numEntries+1];
		for(int s = 0; s < m_numEntries; s++)
		{
			codeWords[s] = m_codeWords[s];
		}
		codeWords[m_numEntries] = new CodeWord;
		if(m_numEntries > 0)
			delete[] m_codeWords;
		codeWords[m_numEntries]->valueRGB[0] = pixel[0];
		codeWords[m_numEntries]->valueRGB[1] = pixel[1];
		codeWords[m_numEntries]->valueRGB[2] = pixel[2];
		codeWords[m_numEntries]->minIntensity = inten;
		codeWords[m_numEntries]->maxIntensity = inten;
		codeWords[m_numEntries]->frequency = 1;
		codeWords[m_numEntries]->maxNegRunLen = m_time - 1;
		codeWords[m_numEntries]->firstVisity = m_time;
		codeWords[m_numEntries]->lastVisity = m_time;
		m_codeWords = codeWords;
		m_numEntries += 1;
	}
}

void CodeBook::ClearStaleEntries()
{
	int staleThresh = m_time >> 1;
	int *keep = new int[m_numEntries];
	int keepCount = 0;
	for(int i = 0; i < m_numEntries; i++)
	{
		int negRun = m_time - m_codeWords[i]->lastVisity + m_codeWords[i]->firstVisity -1;
		if(m_codeWords[i]->maxNegRunLen < negRun)
			m_codeWords[i]->maxNegRunLen = negRun;
		if(m_codeWords[i]->maxNegRunLen > staleThresh)
		{
			keep[i] = 0;
		}
		else
		{
			keep[i] = 1;
			keepCount += 1;
		}
	}
	m_time = 0;
	CodeWord **codeWords = new CodeWord*[keepCount];
	int k = 0;
	for(int i = 0; i < m_numEntries; i++)
	{
		if(keep[i])
		{
			codeWords[k] = m_codeWords[i];
			k++;
		}
	}
	delete[] keep;
	delete[] m_codeWords;
	m_codeWords = codeWords;
	m_numEntries = keepCount;
}

void CodeBook::CreateModel(Mat *frames, unsigned int frameCount, float threshhold, float alph, float beta)
{
	//uchar pixel[CHANNELS];
	for(unsigned int f = 0; f < frameCount; f++)
	{
		for(unsigned int i = 0; i < frames[f].rows; i++)
		{
			const uchar *fi = frames[f].data + i*frames[f].elemSize()*frames[f].cols;
			for(unsigned int j = 0; j < frames[f].cols; j++)
			{
				UpdateCodeBook(fi+j, threshhold, alph, beta);
			}
		}
	}
	ClearStaleEntries();
}

uchar CodeBook::BackgroundDiff(const uchar *pixel, float threshhold, float alph, float beta)
{
	m_time++;
	float inten = sqrt(float(pixel[0]*pixel[0] + pixel[1]*pixel[1] + pixel[2]*pixel[2]));
	int i;
	for(i = 0; i < m_numEntries; i++)
	{
		float delta = ColorDist(pixel, *m_codeWords[i]);
		if(delta <= threshhold && Brightness(*m_codeWords[i], inten, alph, beta))
		{
							
			m_codeWords[i]->valueRGB[0] = (m_codeWords[i]->frequency*m_codeWords[i]->valueRGB[0] + pixel[0]) / (m_codeWords[i]->frequency + 1);
			m_codeWords[i]->valueRGB[1] = (m_codeWords[i]->frequency*m_codeWords[i]->valueRGB[1] + pixel[1]) / (m_codeWords[i]->frequency + 1);
			m_codeWords[i]->valueRGB[2] = (m_codeWords[i]->frequency*m_codeWords[i]->valueRGB[2] + pixel[2]) / (m_codeWords[i]->frequency + 1);
			m_codeWords[i]->minIntensity = inten < m_codeWords[i]->minIntensity?inten : m_codeWords[i]->minIntensity;
			m_codeWords[i]->maxIntensity = inten > m_codeWords[i]->maxIntensity?inten : m_codeWords[i]->maxIntensity;
			m_codeWords[i]->frequency += 1;
			m_codeWords[i]->maxNegRunLen = m_codeWords[i]->maxNegRunLen > (m_time-m_codeWords[i]->lastVisity)?m_codeWords[i]->maxIntensity : (m_time-m_codeWords[i]->lastVisity);
			m_codeWords[i]->lastVisity = m_time;						
			return 0;
		}
	}
	if(i >= m_numEntries)
	{
		return 255;
	}
}